#include <vector>
#include <algorithm>
#include <ros/ros.h>
#include <cmath>
#include <math.h>
#include <deque>
#include <mutex>
#include <thread>
#include <fstream>
#include <csignal>

#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/SetMavFrame.h>

#include <geometry_msgs/TwistStamped.h>
 
#include <geometry_msgs/QuaternionStamped.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <sensor_msgs/NavSatFix.h>
#include <std_msgs/Float32.h>
#include <std_msgs/UInt8.h>
#include <std_msgs/Bool.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <tf/tf.h>
#include <sensor_msgs/LaserScan.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/Joy.h>
#include <sensor_msgs/Imu.h> 
#include <std_msgs/Float32MultiArray.h>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
// #include <sensor_msgs/LaserScan.h>  qaq
#include <mavros_msgs/RCIn.h>
#include <mavros_msgs/OverrideRCIn.h>
#include <mavros_msgs/AttitudeTarget.h>
#include <mavros_msgs/Altitude.h>
#include <mavros_msgs/ActuatorControl.h>
#include <mavros_msgs/WaypointList.h>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/common/common.h>
#include <pcl/filters/voxel_grid.h>
#include <lidar_detect/CustomMsg.h>
#include <unionsys_core.hpp>

#include <visualization_msgs/Marker.h>


#include <swarm_msgs/BoundingBoxes.h>
#include <swarm_msgs/BoundingBox.h>

#include <liblas/liblas.hpp>



RegisterMSN *unionsys_core = new RegisterMSN();

const float deg2rad = 3.1415926535798/180.0;
const float rad2deg = 180.0/3.1415926535798;

//launch file variables
bool use_sitl;
double ClusterTolerance;
double MinClusterSize;
double MaxClusterSize;

double imu_time_offset;

bool use_lio;

int image_width;
int image_height;

struct Box
{
	float x_min;
	float y_min;
	float z_min;
	float x_max;
	float y_max;
	float z_max;
	int id;
	double time;
	bool empty;
};

//ros node define
ros::Subscriber altitude_sub,local_odom_sub;
ros::Subscriber state_sub;
ros::Subscriber huowu_yolo_sub,circle_yolo_sub, standard_cloud_sub, livox_cloud_sub;


ros::Publisher  marker_pub, target_pub, point_cloud_pub1,point_cloud_pub2, marker_id_pub, huowu_pub;


ros::Timer cluster_timer;

//msg define
mavros_msgs::State current_state;
geometry_msgs::Point current_point;
geometry_msgs::Quaternion current_angle;
geometry_msgs::Vector3 curr_plane_angle;
Eigen::Quaterniond current_angle_Q;

std::deque<Box> target_box;
int clusterId = 0;
std::deque<Box> box_list(30);
double filter_intensity;
//point cloud define
std::deque<nav_msgs::Odometry::Ptr> imu_buffer;

std::deque<double>                     time_buffer;
std::deque<pcl::PointCloud<pcl::PointXYZI>::Ptr>        lidar_buffer;

// pcl::PointCloud<pcl::PointXYZI>::Ptr global_cloud_ned(new pcl::PointCloud<pcl::PointXYZI>);
// pcl::PointCloud<pcl::PointXYZI>::Ptr last_global_cloud_ned(new pcl::PointCloud<pcl::PointXYZI>);
// pcl::PointCloud<pcl::PointXYZI>::Ptr last_last_global_cloud_ned(new pcl::PointCloud<pcl::PointXYZI>);
// std::deque<pcl::PointCloud<pcl::PointXYZI>::Ptr>        map_buffer;
pcl::PointCloud<pcl::PointXYZI> global_cloud_ned;
pcl::PointCloud<pcl::PointXYZI> last_global_cloud_ned;
pcl::PointCloud<pcl::PointXYZI> last_last_global_cloud_ned;

pcl::PointCloud<pcl::PointXYZI>::Ptr global_cloud(new pcl::PointCloud<pcl::PointXYZI>);



//flag define
bool local_odom_flag = false;
double current_point_z = 0.0;

template <typename T>
T limit(T a, T max)
{
  if(a > max)
    a = max;
  if(a < -max)
    a = -max;
  return a;
}

void deal_detection_to_3dpose(swarm_msgs::BoundingBoxes msg, std::string class_name, double real_size);
void draw_box(Box temp_box, int id, double width, double red, double green, double blue);
void draw_name(Box temp_box,std::string name, int id);
std::deque<Box> sort_box(std::deque<Box> raw_box);
